import rclpy
from rclpy.node import Node
from std_msgs.msg import Int32MultiArray
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from std_msgs.msg import Int32
from tf_transformations import quaternion_from_euler
from nav2_msgs.action import NavigateToPose
from rclpy.action import ActionClient
from geometry_msgs.msg import PoseStamped

import tf_transformations

import threading
import time
import math
from rclpy.executors import MultiThreadedExecutor

import subprocess
import re
import pyaudio  # 保留pyaudio用于设备检查

from agv_common_class import NavigationClient, AngleSubscriber, OdomSubscriber, RotateRobot, AngleNavGoalPoseSend

# 大模型
from spacemit_llm import LLMModel, FCModel

# 音频
from spacemit_audio import find_audio_card, find_playback_card
from spacemit_audio import play_wav_non_blocking, play_wav, play_audio_in_thread
from spacemit_audio import ASRModel
from spacemit_audio import RecAudioThreadPipeLine, RecAudioPipeLine
from spacemit_audio.record_loop import RecAudioDB
asr_model = ASRModel()
from tools.agv import FuncControllerNode, execute_command

# 新增Playsound库
from playsound import playsound
import os

def play_sound_blocking(file_path):
    """使用Playsound阻塞播放音频"""
    try:
        if os.path.exists(file_path):
            playsound(file_path)
        else:
            print(f"音频文件不存在: {file_path}")
    except Exception as e:
        print(f"播放音频失败: {e}")

def play_sound_non_blocking(file_path):
    """使用线程非阻塞播放音频"""
    def play_thread():
        try:
            if os.path.exists(file_path):
                playsound(file_path)
            else:
                print(f"音频文件不存在: {file_path}")
        except Exception as e:
            print(f"播放音频失败: {e}")
    
    thread = threading.Thread(target=play_thread)
    thread.daemon = True
    thread.start()

def run_executor(executor):
    executor.spin()  # 运行 ROS 2 事件循环（不会阻塞主线程）

def main():
    rclpy.init()

    # 查找音频设备（仅用于录音）
    record_device = find_audio_card()
    print(f"使用录音设备索引: {record_device}")

    func_controller_node = FuncControllerNode() # 总控制
    angle_sub_node = AngleSubscriber()  # 声源定位信息订阅
    odom_sub_node = OdomSubscriber()    # 里程计信息订阅
    node_rotate = RotateRobot(odom_sub_node, angle_sub_node)  # 传递订阅节点的引用

    nav_pub = AngleNavGoalPoseSend(angle_sub_node, invert=True)

    # 多线程执行的管理器
    executor = MultiThreadedExecutor()
    executor.add_node(func_controller_node)
    executor.add_node(angle_sub_node)
    executor.add_node(odom_sub_node)
    executor.add_node(node_rotate)
    executor.add_node(nav_pub)

    executor_thread = threading.Thread(target=run_executor, args=(executor,), daemon=True)
    executor_thread.start()

    # 录音功能类初始化 - 使用更安全的参数
    try:
        # 尝试不同的通道数设置
        rec_audio = RecAudioDB(sld=1, min_db=4000, max_time=30, rate=16000, 
                              device_index=record_device, channels=1)
        print("成功初始化录音设备 (单声道)")
    except Exception as e:
        print(f"单声道初始化失败: {e}")
        try:
            # 尝试双声道
            rec_audio = RecAudioDB(sld=1, min_db=4000, max_time=30, rate=16000, 
                                  device_index=record_device, channels=2)
            print("成功初始化录音设备 (双声道)")
        except Exception as e:
            print(f"双声道初始化失败: {e}")
            print("无法初始化录音设备，使用虚拟录音设备")
            # 创建虚拟录音设备 - 修改为更合理的实现
            class DummyRecorder:
                def __init__(self):
                    self.exit_mode = 1  # 默认设置为有声音
                    self.audio_data = b'\x00' * 32000  # 1秒的静音数据
                
                def record_audio(self):
                    print("虚拟录音设备: 返回静音音频")
                    # 模拟真实录音行为，等待1秒
                    time.sleep(1)
                    return self.audio_data
            
            rec_audio = DummyRecorder()

    # 播放启动提示音
    try:
        wav_file_path = "tools/feedback_voice/qidongchenggong.wav"
        play_sound_blocking(wav_file_path)
    except Exception as e:
        print(f"无法播放启动音: {e}")

    # 直接进入唤醒状态
    print("小车已启动，处于唤醒状态，等待指令...")
    
    try:
        wav_file_path = "tools/feedback_voice/awake.wav"
        play_sound_blocking(wav_file_path)
    except Exception as e:
        print(f"无法播放唤醒音: {e}")

    try:
        while rclpy.ok():
            print("等待语音指令...")
            
            try:
                # 开始录制用户声音
                audio_ret = rec_audio.record_audio()
            except Exception as e:
                print(f"录音失败: {e}")
                # 短暂的等待后重试
                time.sleep(1)
                continue

            # 关键修改：检查是否录到了有效声音
            if hasattr(rec_audio, 'exit_mode') and rec_audio.exit_mode == 0:
                print("未检测到有效声音，跳过处理")
                # 播放提示音表示未检测到声音
                try:
                    wav_file_path = "tools/feedback_voice/meitingqingchu.wav"
                    play_sound_blocking(wav_file_path)
                except Exception as e:
                    print(f"无法播放提示音: {e}")
                continue  # 直接返回等待状态
            
            # 检查音频数据是否有效
            if not audio_ret or len(audio_ret) == 0:
                print("录制的音频为空，跳过处理")
                continue
            
            try:
                # 语音转文字
                print("开始语音转文字----------------------")
                text = asr_model.generate(audio_ret)
                print('用户指令: ', text)

                # 模糊匹配
                ret, func_name = execute_command(text, func_controller_node.function_dict_zh)
                if ret:
                    print(f"执行命令: {func_name}")
                    
                    if func_name == 'rotate_in_place':
                        try:
                            wav_file_path = "tools/feedback_voice/haode.wav"
                            play_sound_non_blocking(wav_file_path)
                        except Exception as e:
                            print(f"无法播放反馈音: {e}")
                        func_controller_node.robot_stop_move()

                    if func_name == 'follow_me':
                        try:
                            wav_file_path = "tools/feedback_voice/haodekaishigensui.wav"
                            play_sound_non_blocking(wav_file_path)
                        except Exception as e:
                            print(f"无法播放反馈音: {e}")

                    if func_name == 'come_here':
                        # 使用默认角度（0度）或当前角度
                        if angle_sub_node.angle is not None:
                            nav_pub.angle_sub = angle_sub_node.angle
                        else:
                            nav_pub.angle_sub = 0  # 使用默认角度
                            
                        nav_pub.calc_target_point() # 计算导航的目标点
                        nav_pub.publish_pose()
                        try:
                            wav_file_path = "tools/feedback_voice/zhengzaiqianwang.wav"
                            play_sound_blocking(wav_file_path)
                        except Exception as e:
                            print(f"无法播放反馈音: {e}")
                    
                    try:
                        func_controller_node.function_dict[func_name]()
                    except Exception as e:
                        print(f"执行命令失败: {e}")

                    if func_name == 'move_away':
                        try:
                            wav_file_path = "tools/feedback_voice/zaidengzhe.wav"
                            play_sound_blocking(wav_file_path)
                        except Exception as e:
                            print(f"无法播放反馈音: {e}")

                else:
                    try:
                        wav_file_path = "tools/feedback_voice/meitingqingchu.wav"
                        play_sound_blocking(wav_file_path)
                    except Exception as e:
                        print(f"无法播放反馈音: {e}")

            except Exception as e:
                print(f"处理指令时出错: {e}")
                # 播放错误提示音
                try:
                    wav_file_path = "tools/feedback_voice/meitingqingchu.wav"
                    play_sound_blocking(wav_file_path)
                except Exception as e:
                    print(f"无法播放提示音: {e}")

    except KeyboardInterrupt:
        print("程序终止")

    finally:
        try:
            node_rotate.stop_move()
            time.sleep(0.1)
            executor.shutdown()
            if rclpy.ok():  # 只有在 rclpy 仍然运行时才调用 shutdown
                rclpy.shutdown()
        except Exception as e:
            print(f"关闭时出错: {e}")

if __name__ == '__main__':
    main()